#include <iostream>
#include <fstream>

#include "katana_utils.h"
#include "MatrixUtils.h"
#include "Gesture.h"
#include "IK.h"
#include "offset.h"

#define DEMO_POINTS 2000 // max nb of data points for demonstration (average 10ms/datapoint)

#define REPRO_POINTS 300 // max nb of points for reproduction, increase this is the 
                         // reproduction stops before the end 

#define MS_PER_STEP 100 // speed for reproduction, (time between each keypoint in 
                        // the spline mode : 100 is secure) 
                        // increase to slow down things .. 

#define N_DEMO 3 // number of asked demonstrations .. 
#define N_GAUSSIAN 6 // number of gaussians for the learning (increase for complex gestures)

Matrix4 viOffset;
IKModel * ik;
CLMBase * katana;

/** Grab a demonstration 
 * store the end effector position in the katana referential 
 * in the result matrix and the joints angle (offseted for ik system) 
 * in the joints matrix. 
 */
void demo_absolute(Matrix &result,Matrix &joints)
{
  result.Resize(DEMO_POINTS,3);
  joints.Resize(DEMO_POINTS,6); 
  Vector position,joints_angles,rposition;
  int i=0;
  char c='0';
  std::cout << "press space to start demo" << std::endl;
  while(c!=' ')
    c = getch();
  c='0';
  std::cout << "recording in progress Press space to stop" << std::endl;
  while(c!=' ')
    {
      getAnglesValues(katana,joints_angles);
      joints_angles = offsetAnglesG(joints_angles);
      joints.SetRow(joints_angles,i);
      position = ik->direct(joints_angles);
      result.SetRow(position,i);
      i++;
      if(i==DEMO_POINTS)
	{
	  std::cout << "max length for demo exceeded ..." << std::endl;
	  break;
	}
      c = getch();
    }
  result.Resize(i,3);
  joints.Resize(i,6);
}

int main()
{
  
  ik = new IKModel("katanaG.xml");
  katana = initKatanaG();
  katana->switchRobotOff();
  Gesture * gest;
  Matrix demos[3];
  Matrix demos_abs[3];
  Matrix demos_joints[3];
  int demo_ok = 1;
  // trying to read previous demos 
  for(int i=0;i<N_DEMO;i++)
    {
      char fname[100];
      FILE * testf;
      sprintf(fname,"demo%02d_absolute.txt",i);
      testf = fopen(fname,"r");
      if(testf == NULL)
	{
	  demo_ok = 0;
	  break;
	}
      fclose(testf);
      demos_abs[i] = loadDataFile(fname);

      sprintf(fname,"demo%02d_joints.txt",i);
      testf = fopen(fname,"r");
      if(testf == NULL)
	{
	  demo_ok = 0;
	  break;
	}
      fclose(testf);
      demos_joints[i] = loadDataFile(fname);
    }
   
  // if something got wrong, grab new demos 
  if(!demo_ok)
    {
      std::cout << "can't read demos ... grabbing new ones" << std::endl;
      for(int i=0;i<N_DEMO;i++)
	{
	  char demo_file[100];
	  demo_absolute(demos_abs[i],demos_joints[i]);
	  sprintf(demo_file,"demo%02d_absolute.txt",i);
	  saveDataFile(demo_file,demos_abs[i]);
	  sprintf(demo_file,"demo%02d_joints.txt",i);
	  saveDataFile(demo_file,demos_joints[i]);
	}
    }
  
  gest = new Gesture(1,N_GAUSSIAN); // one reference (absolute) 
                            // ten states gmm

  gest->setdemos(demos_abs,demos_joints,N_DEMO,0);  
                            // feed with the demos  
  gest->train(); // retrain the model 
  gest->saveParams("trained_gesture","trained_gesture_joints");
              
  gest->init_trajectory();
  gest->init_alpha();

  char c='0';
  std::cout << "move to initial position, then press space" << std::endl;
  while(c!=' ')
    c = getch();

  int n_repro_points=0;

  katana->switchRobotOn(); // block the robot

  Vector joint_angles(6),joint_velocity(6);
  Vector referencePosture;

  getAnglesValues(katana,joint_angles);
  
  joint_angles = offsetAnglesG(joint_angles);
  
  referencePosture = joint_angles;

  Vector3 current_position( ik->direct(joint_angles));
  
  current_position.Print();
  
  Vector speed(3);
  Matrix move(REPRO_POINTS,7);

  std::ofstream fr("generated_absolute.txt");

  // trajectory computation loop .. 
  for(int i=0;i<REPRO_POINTS;i++)
    {
      Vector saved_joints(7);

      saved_joints(0) = i*MS_PER_STEP;
      saved_joints.SetSubVector(1,de_offsetAnglesG(joint_angles));
      saved_joints(6) = joint_angles(5);
      move.SetRow(saved_joints,i);

      /** 
       * the intersting part for the reproduction is here !
       */ 

      // using the gesture model to retrieve a velocity from position
      speed =  gest->trajectory_step_hmm(current_position);
      
      current_position += speed;
      
      // using IK to get new joints angles .. 
      referencePosture = gest->getReferencePosture();
      joint_velocity = ik->inverse_step(joint_angles,
				      current_position,
				      3,0.1,referencePosture);
      
      joint_velocity = de_offsetAnglesG_vel(joint_velocity);

      for(int m=0;m<6;m++)
	controle_moteur_vitesse_rads(katana,i,joint_velocity(i));
     
      getAnglesValues(katana,joint_angles);
      joint_angles = offsetAnglesG(joint_angles);
      current_position = ik->direct(joint_angles); 

      for(int i=0;i<3;i++)
	fr << current_position(i) << " ";
      fr << std::endl;
      n_repro_points++;
      if(speed.Norm()<0.01) // if the movement stopped
	{
	  move.Resize(n_repro_points,7);
	  break;
	}
    }
  saveDataFile("joints.txt",move);

  /* once the movement is computed (and stored in the move 
     matrix, it is sent to the katana with a spline movement 
     class */
  /*
  SplineMovement m(move,katana);
  
  m.initMovement();
  m.playMovement(); */
  c='0';
  std::cout << "press space to switch robot off" <<std::endl;
  while(c!=' ')
    c = getch();
  katana->switchRobotOff();
  
}
